//
// Created by hrg on 2019/12/2.
//

#include "device.h"
#include <cstring>
#include <cassert>
#include <servo.h>
#include <ecio.h>
#include <factory.h>
#include <dirent.h>

#include <cobalt/sys/time.h>
#include <boost/foreach.hpp>

void DeviceRTask(void* argv)
{
    assert(argv != NULL);
    Device *device = (Device*)argv;
    device->RunRTask();
}

Device::Device(EcData *ecData, int master)
    : currpos_(nullptr)
    , currvel_(nullptr)
    , targetpos_(nullptr)
    , targetvel_(nullptr)
    , currptr_(nullptr)
    , master_(nullptr)
    , masterid_(master)
    , ecdata_(ecData)
{
    dataPos_ = 0;
    period_ = 0;
    onstatus_ = 0x00;

    switchon_ = false;
    stoptask_ = true;
    linkstatus_ = false;
    starttask_ = false;
}

Device::~Device()
{
    if(master_ != nullptr)
    {
        delete master_;
        master_ = nullptr;
    }

    if (currpos_ != nullptr)
    {
        delete[] currpos_;
        currpos_ = nullptr;
    }

    if (currvel_ != nullptr)
    {
        delete[] currvel_;
        currvel_ = nullptr;
    }

    if (targetpos_ != nullptr)
    {
        delete[] targetpos_;
        targetpos_ = nullptr;
    }

    if (targetvel_ != nullptr)
    {
        delete[] targetvel_;
        targetvel_ = nullptr;
    }

    servos_.clear();
    ecios_.clear();
}

bool Device::Init(ulong period, ulong offset, int8_t mode, std::string xmldir)
{
    if(master_ != nullptr){
        return false;
    }
    master_ = new Master(masterid_);
    if (!master_->Init())
    {
        return false;
    }

    slvnums_ = master_->GetSlvNum();
    if (slvnums_ == 0)
    {
        return false;
    }

    period_ = period;
    offset_ = offset;
    mode_ = mode;

    int8_t bit[1024];
    int servoid = 0;
    int eciosid = 0;

    memset(bit, 0, sizeof(char) * 1024);
    EcType type = EC_OTHER;
    SlavePtr slave;
    ec_master_t* master = master_->GetMaster();

    struct dirent* entry = nullptr;
    DIR* dir = opendir(xmldir.c_str());
    while(dir) {
        entry = readdir(dir);
        if (entry == nullptr)
			break;
        
        if (entry->d_type != DT_REG)
            continue;

        if (!strstr(entry->d_name, ".xml"))
            continue;

        std::string filename = xmldir + "/" + std::string(entry->d_name);
        if (!m_Ecxml_.open(filename.c_str())) {
            continue;
        }

        for (int i = 0; i < slvnums_; ++i) {
            if ( bit[i] == int8_t(0x01) )
                continue;

            if (!Slave::Match(m_Ecxml_, master, i))
                continue;

            type = Slave::Type(m_Ecxml_, master, i);
            slave = Factory::CreateSlave(type);
            slave->SetType(type);
            slave->Parse(m_Ecxml_, master, i);
            slave->Init(master, i);

            if (type == EC_SERVO) {
                servos_[servoid] = slave;
                servoid += 1;
            } else {
                ecios_[eciosid] = slave;
                eciosid += 1;
            }

            bit[i] = int8_t(0x01);
        }
        m_Ecxml_.close();
    }

    for (int i = 0; i < slvnums_; ++i) {
        if ( bit[i] == int8_t(0x00) ){
            servos_[servoid] = Factory::CreateSlave(EC_SERVO);
            servos_[servoid]->SetType(EC_SERVO);
            servos_[servoid]->Init(master, i);
            servoid += 1;
        }
    }

    currpos_ = new int[servos_.size()];
    currvel_ = new int[servos_.size()];

    targetpos_ = new int[servos_.size()];
    targetvel_ = new int[servos_.size()];

    if (ecdata_ != nullptr)
    {
        ecdata_->Init(servos_.size());
    }

    // init EcStatus
    ecstatus_.actual_pos.resize(servos_.size());
    ecstatus_.actual_vel.resize(servos_.size());
    ecstatus_.target_pos.resize(servos_.size());
    ecstatus_.target_vel.resize(servos_.size());

    if (!ec_status_list_.Init())
    {
        return false;
    }

    return true;
}

bool Device::Config(){
    assert(master_ != nullptr);

    for (int i = 0; i < servos_.size(); ++i) {
        if(servos_[i]){
            if (!servos_[i]->Config(master_->GetDomain()))
            {
                return false;
            }
            servos_[i]->SetDC(period_, offset_, 0, 0);
            servos_[i]->SetMode(mode_);
            servos_[i]->Reset();
        } else {
            std::cout << "[ERROR]: servo[" << i << "]  is nullptr" << std::endl;
        }
    }

    for(int i = 0; i < ecios_.size(); ++i) {
        if (!ecios_[i]->Config(master_->GetDomain()))
        {
            return false;
        }
        ecios_[i]->SetDC(period_, offset_, 0, 0);
        ecios_[i]->SetMode(mode_);
        ecios_[i]->Reset();
    }

    SlavePtr slave;
    if ( servos_.find(0) != servos_.end() ) {
        slave = servos_[0];
    } else if ( ecios_.find(0) != ecios_.end() ) {
        slave = ecios_[0];
    } else {
        return false;
    }

    master_->Config(slave->GetConfig(), period_ / 1000);
    if (!master_->StartMaster())
    {
        std::cerr << "[ERROR]: start master failed" << std::endl;
        return false;
    }

    domainptr_ = master_->GetDomainData();
    return true;
}

void Device::SetSlaveMode(int index, int mode) {
    if (servos_.find(index) == servos_.end()){
        return;
    }
    servos_[index]->SetMode(mode);
}

uint16_t Device::GetErrorCode(int index) {
    if (servos_.find(index) == servos_.end()){
        std::cout << __LINE__ << " warning: "<< index << " servo!!!" << std::endl;
        return false;
    }

    return servos_[index]->GetErrorCode();
}

EcStatus& Device::GetEcStatus()
{
    for(int i = 0; i < servos_.size(); ++ i)
    {
        ecstatus_.actual_pos[i] = servos_[i]->GetActualPos();
        ecstatus_.actual_vel[i] = servos_[i]->GetActualVel();

        ecstatus_.target_pos[i] = servos_[i]->GetTargetPos();
        ecstatus_.target_vel[i] = servos_[i]->GetTargetVel();
    }
    ecstatus_.state = CheckSlvStatus();
    return ecstatus_;
}

int8_t Device::GetOneStatus(int index){
    if (servos_.find(index) == servos_.end()){
        std::cout << __LINE__ << " warning: "<< index << " servo!!!\n" << std::endl;
        return false;
    }

    return servos_[index]->GetState();
}

int8_t Device::GetAllStatus(){
    return onstatus_;
}

EcType Device::GetSlvType(int index){
    if (servos_.find(index) != servos_.end()){
        return EC_SERVO;
    }

    if (ecios_.find(index) != ecios_.end()) {
        return ecios_[index]->GetType();
    }

    return EC_OTHER;
}

bool Device::SetIOCmd(int index, int32_t cmd){
    if ( ecios_.find(index) == ecios_.end() ){
        std::cout << __LINE__ << " warning: "<< index << " servo!!!\n" << std::endl;
        return false;
    }

    ecios_[index]->SetPos(cmd);
    return true;
}
#if 0
uint32_t Device::GetIOCmd(int index){
    if ( ecios_.find(index) == ecios_.end() ) {
        std::cout << __LINE__ << " warning: "<< index << " servo!!!\n" << std::endl;
        return 0x00;
    }

    return ecios_[index]->GetCurrPos();
}
#endif

uint32_t Device::GetIOAna(int index, int pdo)
{
    if ( ecios_.find(index) == ecios_.end() ) {
        std::cout << __LINE__ << " warning: "<< index << " servo!!!\n" << std::endl;
        return 0x00;
    }

    return std::dynamic_pointer_cast<EcIO>(ecios_[index])->GetAnalog(pdo);
}

uint32_t Device::GetIODig(int index)
{
    if ( ecios_.find(index) == ecios_.end() ) {
        std::cout << __LINE__ << " warning: "<< index << " servo!!!\n" << std::endl;
        return 0x00;
    }

    return std::dynamic_pointer_cast<EcIO>(ecios_[index])->GetDigital();
}

int8_t Device::CheckSlvStatus(){
    int8_t onbit = 0x00;
    int8_t enbit = 0x00;

    for (int i = 0; i < servos_.size(); ++i) {
        onbit |= servos_[i]->GetState();

        if (!IsEnableSlaveCheck(i))
            continue;

        enbit |= servos_[i]->GetState();
    }

    if ( enbit == 0x00) {
        if (onbit >= 0x08){
            return 0x08;
        } else {
            return onbit;
        }
    } else if (enbit >= 0x08) {
        return 0x08;
    } else {
        return enbit;
    }
}

void Device::ClearDataCache() {
    if (ecdata_ != nullptr)
    {
        ecdata_->Clear();
    }

    currptr_ = nullptr;
    dataPos_ = 0;
}

bool Device::UpDataPos(){
    if (ecdata_ == nullptr)
        return false;

    if (currptr_ != nullptr) {
        for (int i = 0; i < servos_.size(); ++i, ++dataPos_) {
            if ( !IsEnableSlaveCheck(i))
                continue;

            servos_[i]->SetPos(currptr_[dataPos_]);
        }
    }

    if(dataPos_ == ecdata_->group_count_ * ecdata_->servo_count_){
        currptr_ = nullptr;
        dataPos_ = 0;
    }

    if(currptr_ == nullptr){
        ecdata_->DataBlockTransfToCurPtr();
        currptr_ = ecdata_->DataLoadFromCurrBlock();
    }
    return true;
}

void Device::UpdatePos(Target target)
{
    int8_t status = CheckSlvStatus();
    if (status != 0x04 || !target.size())
    {
        return;
    }

    for (int i = 0; i < servos_.size(); ++i)
    {
        if (!IsEnableSlaveCheck(i))
        {
            continue;
        }

        servos_[i]->SetPos(target[i]);
    }
}

#ifdef DELAY
void Device::SetTaskDelay()
{
    static RTIME prev_time = 0;
    static RTIME curr_time = 0;

    static RTIME max_delay = 0;
    static RTIME min_delay = 1000 * 1000 * 1000;

    static RTIME curr_max_delay = 0;
    static RTIME curr_min_delay = 1000 * 1000 * 1000;

    static bool first = true;

    static int count = 0;

    curr_time = rt_timer_read();

    if (!first)
    {
        RTIME delay = curr_time - prev_time;

        if (delay > curr_max_delay)
        {
            curr_max_delay = delay;
        }

        if (delay < curr_min_delay)
        {
            curr_min_delay = delay;
        }

        if (count >= 5000)
        {
            if (curr_max_delay > max_delay)
            {
                max_delay = curr_max_delay;
            }

            if (curr_min_delay < min_delay)
            {
                min_delay = curr_min_delay;
            }

            std::cout << "curr: " << curr_max_delay << ", " << curr_min_delay << \
                         ", last: " << max_delay << ", " << min_delay << std::endl;

            curr_max_delay = 0;
            curr_min_delay = 1000 * 1000 * 1000;

            count = 0;
        }
        else
        {
            count ++;
        }
    }
    else
    {
        first = false;
    }
    prev_time = curr_time;
}
#endif
void Device::RunRTask(){
    SlavePtr slave;
    rt_task_set_mode(0, T_CONFORMING, nullptr);
    rt_task_set_periodic(nullptr, TM_NOW, period_);
    EcStatus status;
    /**
     * struct timespec now;
     * clock_gettime(CLOCK_COPPERPLATE, &now)
     **/
    while(1) {
        rt_task_wait_period(nullptr);

#ifdef DELAY
        SetTaskDelay();
#endif
        if ( stoptask_ )
            break;

        master_->RecvFrame();

        if ( !linkstatus_ ) {
            if ( master_->CheckLinkStatus() )
                linkstatus_ = true;
            else
                goto send;
        }

        if (!master_->CheckSlaveState()){
            onstatus_ = 0x09;
            if (update_handle_) {
                status = GetEcStatus();
                status.state = onstatus_;
                update_handle_(status);
            }
            break;
        }

        for (int i = 0; i < servos_.size(); ++i) {
            servos_[i]->Retrieve(domainptr_);
            servos_[i]->StatusTransite();
        }

        for (int i = 0; i < ecios_.size(); ++i) {
            ecios_[i]->Retrieve(domainptr_);
            ecios_[i]->StatusTransite();
        }

        status = GetEcStatus();
        ec_status_list_.PushData(status);

        if (update_handle_)
        {
            onstatus_ = ecstatus_.state;
            UpdatePos(update_handle_(status));
        }
        else
        {
            onstatus_ = CheckSlvStatus();
            switch (onstatus_)
            {
            case 0x04:
                UpDataPos();
                break;
            case 0x02:
                switchon_ = true;
                break;
            case 0x08:
                switchon_ = false;
                for(int i = 0; i < servos_.size(); ++ i)
                {
                    DisableSlaves(i);
                }
                break;
            default:
                break;
            }
        }

        for (int i = 0; i < servos_.size(); ++i)
        {
            servos_[i]->Publisher(domainptr_);
        }

        for (int i = 0; i < ecios_.size(); ++i)
        {
            ecios_[i]->Publisher(domainptr_);
        }
send:
        master_->SendFrame();
    }
}

size_t Device::GetServoNum() {
    return servos_.size();
}

size_t Device::GetEcIONum() {
    return ecios_.size();
}

int* Device::GetCurrPos() {
    if (!linkstatus_)
        return nullptr;

    for(int i = 0; i < servos_.size(); ++ i)
    {
        currpos_[i] = servos_[i]->GetActualPos();
    }

    return currpos_;
}

int* Device::GetCurrVel()
{
    if (!linkstatus_)
        return nullptr;

    for (int i = 0; i < servos_.size(); ++ i)
    {
        currvel_[i] = servos_[i]->GetActualVel();
    }

    return currvel_;
}

int* Device::GetTargetPos()
{
    if (!linkstatus_)
        return nullptr;

    for(int i = 0; i < servos_.size(); ++ i)
    {
        targetpos_[i] = servos_[i]->GetTargetPos();
    }

    return targetpos_;
}

int* Device::GetTargetVel()
{
    if (!linkstatus_)
        return nullptr;

    for(int i = 0; i < servos_.size(); ++ i)
    {
        targetvel_[i] = servos_[i]->GetTargetVel();
    }

    return targetvel_;
}

bool Device::StartRTask() {
    assert(master_ != nullptr);
    stoptask_ = false;
#if 0
    //! 将当前的Linux任务转换为Xenomai任务
    if (rt_task_shadow(&rtask_, "devicertask", 99, T_FPU)){
        return false;
    }
#endif
    if (rt_task_create(&rtask_, "DeviceRTask", 0, 99, T_FPU)) {
        std::cerr << "Failed to create DeviceRTask task!!!!" << std::endl;
        return false;
    }

    if (rt_task_start(&rtask_, &DeviceRTask, this)) {
        std::cerr << "Failed to start DeviceRTask Task!!!!" << std::endl;
        return false;
    }

    starttask_ = true;
    return true;
}

void Device::Reset(int index){
    if (servos_.find(index) == servos_.end()){
        std::cout << "warning: "<< index << " servo!!!\n" << std::endl;
        return;
    }
    servos_[index]->Reset();
}

bool Device::StopRTask(){
    assert(master_ != nullptr);
    if (!starttask_) {
        return true;
    }

    for (int i = 0; i < servos_.size(); ++i) {
        DisableSlaves(i);
    }

    while (1) {
        rt_task_yield();
#if 0
        if ( onstatus_ == 0x00 ){
            stoptask_ = true;
            break;
        }

        if ( onstatus_ == 0x02 ) {
            stoptask_ = true;
            break;
        }

        if ( onstatus_ & 0x08 ) {
            stoptask_ = true;
            break;
        }
#else
        if (onstatus_ != 0x04) {
            stoptask_ = true;
            break;
        }
#endif
    }

    if ( starttask_ ) {
        if (onstatus_ != 0x09){
            rt_task_join(&rtask_);
        }
        rt_task_delete(&rtask_);
        starttask_ = false;
    }
    return true;
}

bool Device::EnableSlaves(int index)
{
    if (servos_.find(index) == servos_.end()){
        std::cout << __LINE__ << " warning: "<< index << " servo!!!\n" << std::endl;
        return false;
    }

    servos_[index]->SetEnable(true);
    return true;
}

bool Device::DisableSlaves(int index)
{
    if (servos_.find(index) == servos_.end()){
        std::cout << __LINE__ << "warning: "<< index << " servo!!!\n" << std::endl;
        return false;
    }

    servos_[index]->SetEnable(false);
    return true;
}

bool Device::IsEnableSlaveCheck(int index)
{
    if (servos_.find(index) == servos_.end()){
        std::cout << __LINE__ << "warning: "<< index << " servo!!!\n" << std::endl;
        return false;
    }

    return servos_[index]->GetEnable();
}
